#pragma once

#ifndef _FRONTEND_H_
#define _FRONTEND_H_

#include "frame.h"
#include"config.h"
#include"map.h"
#include"feature_extract.h"
#include"utility.h"
#include"imu_handle.h"

namespace vld_imu_slam{

enum class  FrontendStatus { INITING, TRACKING_GOOD, TRACKING_BAD, LOST };

class Frontend{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Frontend> Ptr;

    Config::Ptr p_config_;
    Frame::Ptr p_last_frame_,p_current_frame_;

    Map::Ptr p_map_;

    IMUHandle::Ptr p_imu_handle_;

    Frontend(){}
    Frontend(Config::Ptr p_config){
        p_config_ = p_config;
        p_map_.reset(new Map);
        laserCloudOri_.reset(new pcl::PointCloud<PointType>);
        coeffSel_.reset(new pcl::PointCloud<PointType>);

        q_last_cur_ = Eigen::Quaterniond(1,0,0,0);
        t_last_cur_ = Eigen::Vector3d(0,0,0);
        test_rpy_ = Eigen::Vector3d(0,0,0);
        matP_ = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));

        p_imu_handle_.reset(new IMUHandle(p_config));
    }

    void addFrame(shared_ptr<Frame> p_frame);

    bool frontendHandle();

private:
    FrontendStatus status_ = FrontendStatus::INITING;

    //track
    SE3 relative_motion_last_cur_;  // 当前帧与上一帧的相对运动，用于估计当前帧pose初值

    pcl::KdTreeFLANN<PointType>::Ptr p_kdtree_surface_,p_kdtree_corner_;
    pcl::PointCloud<PointType>::Ptr laserCloudOri_;
    pcl::PointCloud<PointType>::Ptr coeffSel_;
    double max_dis_plane_for_match_ = 1;
    Eigen::Quaterniond q_last_cur_;
    Eigen::Vector3d t_last_cur_;
    bool is_degenerate_ = false;
    vector<pair<Eigen::Vector3d,Eigen::Vector3d>>  plane_p_normal_set_;

    unordered_map<int, pair<Eigen::Vector3d,Eigen::Vector3d>> line_pa_pb_set_;

    // Eigen::Matrix<double, 6, 6> matP_ = Eigen::Matrix<double, 6, 6>::Zero();
    cv::Mat matP_;

    Eigen::Vector3d test_rpy_,last_kf_rpy_,last_kf_t_;

    deque<Frame::Ptr> frame_deque_;

    mutex frame_mutex_;

    mutex cal_resd_mutex_;

    void init();

    void frameRegistration();

    void calSurfResidual();

    // bool calSurfJacobi(int iterCount);

    void calCornerResidual();

    bool LMOptimization(int iterCount);

    // bool calCornerJacobi(int iterCount);

    bool judgeKeyframe();

};



}
#endif  
